#include "CollisionDetector.h"

CollisionDetector::CollisionDetector(vector<Polygon_2>	robot_polygons, Obstacles *obs): 
			m_robot_polygons(robot_polygons), m_obs(obs) 
{
	// Construction of the polygon set, used for collision detection
	if (!m_obs->empty())
	{
		for (Obstacles::iterator iter = m_obs->begin(); iter != m_obs->end(); iter++)
		{
			// For every obstacle calculate its Minkowski sum with the "robot"
			Polygon_with_holes_2	poly_wh = minkowski_sum_2(*iter, negative_polygon(m_robot_polygons[0]));
			Polygon_with_holes_2	poly_wh2 = minkowski_sum_2(*iter, negative_polygon(m_robot_polygons[1]));
				
			// Add the result to the polygon set
			m_robot_1_poly_set.join(poly_wh);
			m_robot_2_poly_set.join(poly_wh2);
		}
	}
		
	Polygon_with_holes_2 robots_poly_wh = minkowski_sum_2(m_robot_polygons[1], negative_polygon(m_robot_polygons[0]));
	m_robots_poly_set.join(robots_poly_wh);
}
	
Polygon_2 CollisionDetector::negative_polygon(Polygon_2 polygon)
{
	Polygon_2 negative_robot;
	for (Polygon_2::Vertex_iterator verteces_it = polygon.vertices_begin();
		verteces_it != polygon.vertices_end();
		++verteces_it)
	{
		negative_robot.push_back(CGAL::ORIGIN - Vector_2(CGAL::ORIGIN, (*verteces_it)));
	}

	return negative_robot;
}

/*	Check if the given configuration is collision free (returns true for free samples) */
bool CollisionDetector::single_robot_valid_conf(int index, Conf robot_conf)
{
	if (index == 1)
	{
		return ((m_robot_1_poly_set.oriented_side(robot_conf) == CGAL::ON_NEGATIVE_SIDE));
	}

	return ((m_robot_2_poly_set.oriented_side(robot_conf) == CGAL::ON_NEGATIVE_SIDE));
}

/*	Check if the given configuration is collision free (returns true for free samples) */
bool CollisionDetector::valid_conf(Conf robot_1_conf, Conf robot_2_conf)
{
	//cout << "valid_conf (" << robot_1_conf.x() << "," << robot_1_conf.y() << ") (" << robot_2_conf.x() << "," << robot_2_conf.y() << ")\n";
	return (((m_robot_1_poly_set.oriented_side(robot_1_conf) == CGAL::ON_NEGATIVE_SIDE)) &&
			((m_robot_2_poly_set.oriented_side(robot_2_conf) == CGAL::ON_NEGATIVE_SIDE)) &&
			((m_robots_poly_set.oriented_side(robot_1_conf - Vector_2(CGAL::ORIGIN, robot_2_conf)) == CGAL::ON_NEGATIVE_SIDE)));
}

bool CollisionDetector::single_robot_local_planner(int index, Point_2 start, Point_2 target, double step_size)
{
	double robot_x1 = CGAL::to_double(start.x());
	double robot_y1 = CGAL::to_double(start.y());
	double robot_x2 = CGAL::to_double(target.x());
	double robot_y2 = CGAL::to_double(target.y());

	double robot_distance = sqrt(pow(robot_x1 - robot_x2, 2) + pow(robot_y1 - robot_y2,2));

	// calculate how many steps are required
	int robot_step_num = floor((robot_distance - step_size) / step_size);
	double robot_vx = robot_x2 - robot_x1;
	double robot_vy = robot_y2 - robot_y1;

	double robot_partial_offset = step_size / (robot_distance - step_size);

	for (int i = 1; i <= robot_step_num; i++)
	{
		//cout << "local_planner index - " << i << "/" << stemp_num << "\n";
		// generate a configuration for every step
		double robot_offset =  i * robot_partial_offset;
		double robot_currx = robot_x1 + robot_vx * robot_offset;
		double robot_curry = robot_y1 + robot_vy * robot_offset;

		Point_2 robot_current_conf(robot_currx, robot_curry);

		// If an intermidiate configuration is invalid, return false
		if (!single_robot_valid_conf(index, robot_current_conf))
		{
			//cout << "no path\n";
			return false;
		}
	}

	// GREAT SUCCESS!
	//cout << "there's path\n";
	return true;
}
bool CollisionDetector::local_planner(ConfSet start, ConfSet target, double step_size)
{
	//cout << "local_planner\n";
	Point_2 robot_1_start = start[0];
	Point_2 robot_2_start = start[1];
	Point_2 robot_1_target = target[0];
	Point_2 robot_2_target = target[1];

	double robot_1_x1 = CGAL::to_double(robot_1_start.x());
	double robot_1_y1 = CGAL::to_double(robot_1_start.y());
	double robot_1_x2 = CGAL::to_double(robot_1_target.x());
	double robot_1_y2 = CGAL::to_double(robot_1_target.y());

	double robot_1_distance = sqrt(pow(robot_1_x1 - robot_1_x2, 2) + pow(robot_1_y1 - robot_1_y2,2));

	// calculate how many steps are required
	int robot_1_step_num = floor((robot_1_distance - step_size) / step_size);
	double robot_1_vx = robot_1_x2 - robot_1_x1;
	double robot_1_vy = robot_1_y2 - robot_1_y1;

	double robot_2_x1 = CGAL::to_double(robot_2_start.x());
	double robot_2_y1 = CGAL::to_double(robot_2_start.y());
	double robot_2_x2 = CGAL::to_double(robot_2_target.x());
	double robot_2_y2 = CGAL::to_double(robot_2_target.y());

	double robot_2_distance = sqrt(pow(robot_2_x1 - robot_2_x2, 2) + pow(robot_2_y1 - robot_2_y2,2));

	// calculate how many steps are required
	int robot_2_step_num = floor((robot_2_distance - step_size) / step_size);
	double robot_2_vx = robot_2_x2 - robot_2_x1;
	double robot_2_vy = robot_2_y2 - robot_2_y1;

	int stemp_num = (robot_2_step_num < robot_1_step_num) ? robot_1_step_num : robot_2_step_num;

	double robot_1_partial_offset = step_size / (robot_1_distance - step_size);
	double robot_2_partial_offset = step_size / (robot_2_distance - step_size);

	for (int i = 1; i <= stemp_num; i++)
	{
		//cout << "local_planner index - " << i << "/" << stemp_num << "\n";
		// generate a configuration for every step
		double robot_1_offset =  i * robot_1_partial_offset;
		double robot_1_currx = robot_1_x1 + robot_1_vx * robot_1_offset;
		double robot_1_curry = robot_1_y1 + robot_1_vy * robot_1_offset;

		Conf robot_1_current_conf(robot_1_currx, robot_1_curry);

		// generate a configuration for every step
		double robot_2_offset =  i * robot_2_partial_offset;
		double robot_2_currx = robot_2_x1 + robot_2_vx * robot_2_offset;
		double robot_2_curry = robot_2_y1 + robot_2_vy * robot_2_offset;

		Conf robot_2_current_conf(robot_2_currx, robot_2_curry);

		// If an intermidiate configuration is invalid, return false
		if (!valid_conf(robot_1_current_conf, robot_2_current_conf))
		{
			//cout << "no path\n";
			return false;
		}
	}

	// GREAT SUCCESS!
	//cout << "there's path\n";
	return true;
}

Polygon_2 CollisionDetector::get_new_polygon( Conf new_robot_point, Polygon_2 original_robot ) 
{
	Polygon_2 new_polygon;
	for (Polygon_2::Vertex_iterator v_it = original_robot.vertices_begin();
			v_it != original_robot.vertices_end();
			++v_it)
	{
		Point_2 new_point(v_it->x() + new_robot_point.x(),v_it->y() + new_robot_point.y());
		new_polygon.push_back(new_point);
	}

	return new_polygon;
}